#ifndef POINT_CLOUD_H_
#define POINT_CLOUD_H_

#include <array>
#include <cstdint>
#include <cstring>

#include "ad_exception.h"

// Point cloud
template <class T, std::uint32_t size>
class PointCloud {
 public:
  PointCloud() noexcept : num_points_{0U} {}
  ~PointCloud() noexcept = default;
  PointCloud(PointCloud const& point_cloud) {
    memcpy(points_.data(), point_cloud.points_.data(),
           point_cloud.num_points_ * sizeof(T));
    num_points_ = point_cloud.num_points_;
  }
  PointCloud& operator=(PointCloud const& point_cloud) {
    memcpy(points_.data(), point_cloud.points_.data(),
           point_cloud.num_points_ * sizeof(T));
    num_points_ = point_cloud.num_points_;
    return *this;
  }
  PointCloud(PointCloud&&) = default;
  PointCloud& operator=(PointCloud&&) = default;

  /// @brief Get number of points
  ///
  /// @return num_points_
  std::uint32_t GetPointNum() const noexcept { return num_points_; }

  /// @brief Get point by index
  ///
  /// @param index  point index
  /// @throw AdException  when out of range
  ///
  /// @return num_points_
  T const& GetPoint(std::uint32_t index) const noexcept(false) {
    if (index >= num_points_) {
      throw AdException("Point not exist");
    }
    return points_[index];
  }

  /// @brief Get array for data logging
  ///
  /// @return points_
  std::array<T, size>& GetArray() noexcept { return points_; }

  /// @brief Set point by index
  ///
  /// @param index  point index
  /// @param point  point
  /// @throw AdException  when out of range
  void SetPoint(std::uint32_t index, T const& point) noexcept(false) {
    if (index >= num_points_) {
      throw AdException("Point not exist");
    }
    points_[index] = point;
  }

  /// @brief Set number of points
  ///
  /// @param num_points  number of points
  /// @throw AdException  when point cloud is too large
  void SetPointNum(uint32_t num_points) noexcept(false) {
    if (num_points > size) {
      throw AdException("Point cloud is too large");
    }
    num_points_ = num_points;
  }

  /// @brief Push back a point
  ///
  /// @param point  point
  /// @throw AdException  when point cloud is full
  void AddPoint(T const& point) noexcept(false) {
    if (num_points_ >= size) {
      throw AdException("Point cloud is full");
    }
    points_[num_points_] = point;
    num_points_++;
  }

  /// @brief Join a point cloud
  ///
  /// @param cloud  point cloud to join
  /// @throw AdException  when point cloud is full
  template <std::uint32_t join_size>
  void Join(PointCloud<T, join_size> const& cloud) noexcept(false) {
    if (num_points_ + cloud.num_points_ > size) {
      throw AdException("Point cloud not large enough");
    }
    memcpy(&(points_[num_points_]), cloud.points_.data(),
           cloud.num_points_ * sizeof(T));
    num_points_ += cloud.num_points_;
  }

  /// @brief Clear point cloud
  void Clear() noexcept { num_points_ = 0U; }

 private:
  /// @brief Point array
  std::array<T, size> points_;
  /// @brief Number of points
  std::uint32_t num_points_;
};

#endif  // POINT_CLOUD_H_
